#include "Python.h"
#include "numpy/arrayobject.h"
#include "array.h"

#include <math.h>
#include <stdlib.h>


static PyMethodDef _arrayMethods[] = {
  {"unsafe_sqrt", unsafe_sqrt, METH_VARARGS},
  {"v3_cos_theta", v3_cos_theta, METH_VARARGS},
  {"v3_pt2", v3_pt2, METH_VARARGS},
  {"v3_pt2_single_axis", v3_pt2_single_axis, METH_VARARGS},
  {"v3_unit", v3_unit, METH_VARARGS},
  {"v3_angle", v3_angle, METH_VARARGS},
  {"v3_eta", v3_eta, METH_VARARGS},
  {"v3_rotate_x", v3_rotate_x, METH_VARARGS},
  {"v3_rotate_y", v3_rotate_y, METH_VARARGS},
  {"v3_rotate_z", v3_rotate_z, METH_VARARGS},
  {"v4_boost_arr", v4_boost_arr, METH_VARARGS},
  {"v4_boost_onevec", v4_boost_onevec, METH_VARARGS},
  {"v4_et2", v4_et2, METH_VARARGS},
  {"v4_et2_single_axis", v4_et2_single_axis, METH_VARARGS},
  {"v4_et2_multi_axis", v4_et2_multi_axis, METH_VARARGS},
  {"v4_gamma", v4_gamma, METH_VARARGS},
  {NULL, NULL}
};

void init_array(void) {
  (void) Py_InitModule("_array", _arrayMethods);

  /* Defined in numpy/arrayobject.h */
  import_array();
}

static PyObject* unsafe_sqrt(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npout;
  int i, dims[2];
  double *xin, *out;

  if (!PyArg_ParseTuple(args, "O!", &PyArray_Type, &npin_x))
    return NULL;

  if (npin_x == NULL)
    return NULL;

  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  out = (double*) npout->data;

  for (i=0; i < dims[0]; i++) {
    if (xin[i] < 0)
      out[i] = sqrt(-xin[i]);
    else
      out[i] = sqrt(xin[i]);
  }
  
  return PyArray_Return(npout);
}


static PyObject* v3_cos_theta(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z, *npout;
  int i, dims[2];
  double *xin, *yin, *zin, *out;

  if (!PyArg_ParseTuple(args, "O!O!O!", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z))
    return NULL;


  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;


  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;
  out = (double*) npout->data;

  for (i=0; i < dims[0]; i++) 
    out[i] = zin[i] / svc_v3_mag(xin[i], yin[i], zin[i]);
  
  return PyArray_Return(npout);
}

static PyObject* v3_pt2(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z;
  PyArrayObject *npin_xax, *npin_yax, *npin_zax;
  PyArrayObject *npout;
  int i, dims[2];
  double *xin, *yin, *zin;
  double *xax, *yax, *zax;
  double *out;

  if (!PyArg_ParseTuple(args, "O!O!O!O!O!O!", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&PyArray_Type, &npin_xax, 
			&PyArray_Type, &npin_yax, 
			&PyArray_Type, &npin_zax))

    return NULL;

  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  if (npin_xax == NULL)
    return NULL;

  if (npin_yax == NULL)
    return NULL;

  if (npin_zax == NULL)
    return NULL;

  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;
  xax = (double*) npin_xax->data;
  yax = (double*) npin_yax->data;
  zax = (double*) npin_zax->data;

  out = (double*) npout->data;

  for (i=0; i < dims[0]; i++) {
    double tot = svc_v3_mag2(xax[i], yax[i], zax[i]);
    double ss  = svc_v3_dot(xin[i], yin[i], zin[i],
			    xax[i], yax[i], zax[i]);
    double per = svc_v3_mag2(xin[i], yin[i], zin[i]);
    if (tot > 0.0)
      per -= ss*ss/tot;
    if (per < 0)
      per = 0;
    
    out[i] = per;
  }
    
  
  return PyArray_Return(npout);
}


static PyObject* v3_pt2_single_axis(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z;
  PyArrayObject *npout;
  int i, dims[2];
  double *xin, *yin, *zin;
  double xax, yax, zax;
  double *out;

  if (!PyArg_ParseTuple(args, "O!O!O!ddd", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&xax, &yax, &zax))          
    return NULL;

  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;

  out = (double*) npout->data;

  double tot = svc_v3_mag2(xax, yax, zax);
  for (i=0; i < dims[0]; i++) {
    double ss  = svc_v3_dot(xin[i], yin[i], zin[i],
			    xax, yax, zax);
    double per = svc_v3_mag2(xin[i], yin[i], zin[i]);
    if (tot > 0.0)
      per -= ss*ss/tot;
    if (per < 0)
      per = 0;
    
    out[i] = per;
  }
    
  
  return PyArray_Return(npout);
}


static PyObject* v3_unit(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z;
  PyArrayObject *npout;
  int i, dims[2];
  double *xin, *yin, *zin;
  double **out; 

  if (!PyArg_ParseTuple(args, "O!O!O!", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z))
    return NULL;

  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  dims[0] = 3;
  dims[1] = npin_x->dimensions[0]; 

  npout = (PyArrayObject*) PyArray_FromDims(2,dims,npin_x->descr->type_num);

  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;

  out = npy2cptr_2d(npout); /*Allocates memory!*/

  for (i=0; i < dims[1]; i++) {    
    double mag = svc_v3_mag(xin[i], yin[i], zin[i]);

    out[0][i] = xin[i] / mag;
    out[1][i] = yin[i] / mag;
    out[2][i] = zin[i] / mag;
  }
    
  free((char*) out);
  
  return PyArray_Return(npout);
}

static PyObject* v3_angle(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x1, *npin_y1, *npin_z1; 
  PyArrayObject *npin_x2, *npin_y2, *npin_z2; 
  PyArrayObject *npout;
  int i, dims[2];
  double *xin1, *yin1, *zin1;
  double *xin2, *yin2, *zin2;
  double *out;

  if (!PyArg_ParseTuple(args, "O!O!O!O!O!O!", 
			&PyArray_Type, &npin_x1, 
			&PyArray_Type, &npin_y1, 
			&PyArray_Type, &npin_z1,
			&PyArray_Type, &npin_x2, 
			&PyArray_Type, &npin_y2, 
			&PyArray_Type, &npin_z2))
    return NULL;


  if (npin_x1 == NULL)
    return NULL;

  if (npin_y1 == NULL)
    return NULL;

  if (npin_z1 == NULL)
    return NULL;

  if (npin_x2 == NULL)
    return NULL;

  if (npin_y2 == NULL)
    return NULL;

  if (npin_z2 == NULL)
    return NULL;

  dims[0] = npin_x1->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x1->descr->type_num);
  
  xin1 = (double*) npin_x1->data;
  yin1 = (double*) npin_y1->data;
  zin1 = (double*) npin_z1->data;

  xin2 = (double*) npin_x2->data;
  yin2 = (double*) npin_y2->data;
  zin2 = (double*) npin_z2->data;

  out = (double*) npout->data;

  for (i=0; i < dims[0]; i++) {
    double ptot2 = svc_v3_mag2(xin1[i], yin1[i], zin1[i]) * svc_v3_mag2(xin2[i], yin2[i], zin2[i]);
    if (ptot2 <= 0.0)
      out[i] = 0.0;
    else {
      double arg = svc_v3_dot(xin1[i], yin1[i], zin1[i],
			      xin2[i], yin2[i], zin2[i]) / sqrt(ptot2);
     
      if (arg > 1.0)
	arg = 1.0;
      else if(arg < -1.0)
	arg = -1.0;
      
      out[i] = acos(arg);
    }
  }

  return PyArray_Return(npout);
}

static PyObject* v3_eta(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z, *npout;
  int i, dims[2];
  double *xin, *yin, *zin, *out;

  if (!PyArg_ParseTuple(args, "O!O!O!", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z))
    return NULL;


  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;


  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;
  out = (double*) npout->data;

  for (i=0; i < dims[0]; i++) {
    double cos_theta = svc_v3_cos_theta(xin[i], yin[i], zin[i]);
    
    if (pow(cos_theta, 2) < 1)
      out[i] = -0.5 * log( (1.0 - cos_theta) / (1.0 + cos_theta) );
    else if (zin[i] > 0)
      out[i] = 1e300;    
    else
      out[i] = -1e300;
  }  

  return PyArray_Return(npout);
}
    
static PyObject* v3_rotate_x(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z;
  int i, dims[2];
  double angle;
  double *xin, *yin, *zin;

  if (!PyArg_ParseTuple(args, "O!O!O!d", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
	                &angle))
    return NULL;


  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;

  dims[0] = npin_x->dimensions[0];

  double s = sin(angle);
  double c = cos(angle);    
  for (i=0; i < dims[0]; i++) {
    double yy = yin[i];
    yin[i] = c*yy - s*zin[i];
    zin[i] = s*yy + c*zin[i];          
  }
  
  Py_INCREF(Py_None);
  return Py_None;  
}

static PyObject* v3_rotate_y(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z;
  int i, dims[2];
  double angle;
  double *xin, *yin, *zin;

  if (!PyArg_ParseTuple(args, "O!O!O!d", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&angle))
    return NULL;


  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;

  dims[0] = npin_x->dimensions[0];

  double s = sin(angle);
  double c = cos(angle);    
  for (i=0; i < dims[0]; i++) {
    double zz = zin[i];
    zin[i] = c*zz - s*xin[i];
    xin[i] = s*zz + c*xin[i];          
  }
  
  Py_INCREF(Py_None);
  return Py_None;  
}

static PyObject* v3_rotate_z(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z;
  int i, dims[2];
  double angle;
  double *xin, *yin, *zin;

  if (!PyArg_ParseTuple(args, "O!O!O!d", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
	                &angle))
    return NULL;


  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;

  dims[0] = npin_x->dimensions[0];

  double s = sin(angle);
  double c = cos(angle);    
  for (i=0; i < dims[0]; i++) {
    double xx = xin[i];
    xin[i] = c*xx - s*yin[i];
    yin[i] = s*xx + c*yin[i];          
  }
  
  Py_INCREF(Py_None);
  return Py_None;  
}

static PyObject* v4_et2(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z, *npin_t;
  PyArrayObject *npout;
  int i, dims[2];
  double *xin, *yin, *zin, *tin;
  double *out;

  if (!PyArg_ParseTuple(args, "O!O!O!O!", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&PyArray_Type, &npin_t))
    return NULL;

  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  if (npin_t == NULL)
    return NULL;
  
  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;
  tin = (double*) npin_t->data;

  out = (double*) npout->data;

  for (i=0; i < dims[0]; i++) {
    double pt2 = xin[i]*xin[i] + yin[i]*yin[i];
    
    out[i] = tin[i]*tin[i] * pt2 / (pt2 + zin[i]*zin[i]);
  }          
  
  return PyArray_Return(npout);
}

static PyObject* v4_et2_single_axis(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z, *npin_t;
  PyArrayObject *npout;
  int i, dims[2];
  double *xin, *yin, *zin, *tin;
  double xax, yax, zax;
  double *out;

  if (!PyArg_ParseTuple(args, "O!O!O!O!ddd", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&PyArray_Type, &npin_t,
			&xax, &yax, &zax))          
    return NULL;

  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;
  tin = (double*) npin_t->data;

  out = (double*) npout->data;

  double tot = svc_v3_mag2(xax, yax, zax);
  double u_xax = xax / sqrt(tot);
  double u_yax = yax / sqrt(tot);
  double u_zax = zax / sqrt(tot);

  for (i=0; i < dims[0]; i++) {
    double ss  = svc_v3_dot(xin[i], yin[i], zin[i],
			    xax, yax, zax);
    double pt2 = svc_v3_mag2(xin[i], yin[i], zin[i]);
    double pv  = svc_v3_dot(xin[i], yin[i], zin[i],
			    u_xax, u_yax, u_zax);
    
    if (tot > 0.0)
      pt2 -= ss*ss/tot;
    if (pt2 < 0)
      pt2 = 0;
       
    out[i] = tin[i]*tin[i] * pt2 / (pt2 + pv*pv);
  }
      
  return PyArray_Return(npout);

}

static PyObject* v4_et2_multi_axis(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z, *npin_t;
  PyArrayObject *npin_xax, *npin_yax, *npin_zax;
  PyArrayObject *npout;
  int i, dims[2];
  double *xin, *yin, *zin, *tin;
  double *xax, *yax, *zax;
  double *out;

  if (!PyArg_ParseTuple(args, "O!O!O!O!O!O!O!", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&PyArray_Type, &npin_t,
			&PyArray_Type, &npin_xax, 
			&PyArray_Type, &npin_yax, 
			&PyArray_Type, &npin_zax))

    return NULL;

  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  if (npin_t == NULL)
    return NULL;

  if (npin_xax == NULL)
    return NULL;

  if (npin_yax == NULL)
    return NULL;

  if (npin_zax == NULL)
    return NULL;

  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;
  tin = (double*) npin_t->data;

  xax = (double*) npin_xax->data;
  yax = (double*) npin_yax->data;
  zax = (double*) npin_zax->data;

  out = (double*) npout->data;

  for (i=0; i < dims[0]; i++) {
    double tot = svc_v3_mag2(xax[i], yax[i], zax[i]);
    double ss  = svc_v3_dot(xin[i], yin[i], zin[i],
			    xax[i], yax[i], zax[i]);
    double pt2 = svc_v3_mag2(xin[i], yin[i], zin[i]);

    double u_xax = xax[i] / sqrt(tot);
    double u_yax = yax[i] / sqrt(tot);
    double u_zax = zax[i] / sqrt(tot);

    double pv  = svc_v3_dot(xin[i], yin[i], zin[i],
			    u_xax, u_yax, u_zax);
  

    if (tot > 0.0)
      pt2 -= ss*ss/tot;
    if (pt2 < 0)
      pt2 = 0;
       
    
    out[i] = tin[i]*tin[i] * pt2 / (pt2 + pv*pv);    
  }
    
  
  return PyArray_Return(npout);
}

static PyObject* v4_boost_arr(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z, *npin_t;
  PyArrayObject *npin_bx, *npin_by, *npin_bz;
  int i, dims[2];
  double *x, *y, *z, *t;
  double *bx, *by, *bz;

  if (!PyArg_ParseTuple(args, "O!O!O!O!O!O!O!", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&PyArray_Type, &npin_t,
			&PyArray_Type, &npin_bx, 
			&PyArray_Type, &npin_by, 
			&PyArray_Type, &npin_bz))

    return NULL;

  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  if (npin_t == NULL)
    return NULL;

  if (npin_bx == NULL)
    return NULL;

  if (npin_by == NULL)
    return NULL;

  if (npin_bz == NULL)
    return NULL;

  dims[0] = npin_x->dimensions[0];
  
  x = (double*) npin_x->data;
  y = (double*) npin_y->data;
  z = (double*) npin_z->data;
  t = (double*) npin_t->data;

  bx = (double*) npin_bx->data;
  by = (double*) npin_by->data;
  bz = (double*) npin_bz->data;

  for (i=0; i < dims[0]; i++) {
    double b2 = bx[i]*bx[i] + by[i]*by[i] + bz[i]*bz[i];
    double gamma = 1.0 / sqrt(1.0 - b2);
    double bp = bx[i]*x[i] + by[i]*y[i] + bz[i]*z[i];
    double gamma2 = b2 > 0 ? (gamma - 1.0)/b2 : 0.0;

    //     SetX(X() + gamma2*bp*bx + gamma*bx*T());
    x[i] = x[i] + gamma2*bp*bx[i] + gamma*bx[i]*t[i];
    
    //SetY(Y() + gamma2*bp*by + gamma*by*T());
    y[i] = y[i] + gamma2*bp*by[i] + gamma*by[i]*t[i];

    //    SetZ(Z() + gamma2*bp*bz + gamma*bz*T());
    z[i] = z[i] + gamma2*bp*bz[i] + gamma*bz[i]*t[i];

    // SetT(gamma*(T() + bp));
    t[i] = gamma*(t[i] + bp);
  }
    
  Py_INCREF(Py_None);
  return Py_None;    
}

static PyObject* v4_boost_onevec(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z, *npin_t;
  int i, dims[2];
  double *x, *y, *z, *t;
  double bx = 0.0;
  double by = 0.0;
  double bz = 0.0;

  if (!PyArg_ParseTuple(args, "O!O!O!O!ddd", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&PyArray_Type, &npin_t,
			&bx, &by, &bz))

    return NULL;

  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  if (npin_t == NULL)
    return NULL;

  if ((bx == 0.0) && (by == 0.0) && (bz == 0.0))
    return NULL;

  dims[0] = npin_x->dimensions[0];
  
  x = (double*) npin_x->data;
  y = (double*) npin_y->data;
  z = (double*) npin_z->data;
  t = (double*) npin_t->data;

  for (i=0; i < dims[0]; i++) {
    double b2 = bx*bx + by*by + bz*bz;
    double gamma = 1.0 / sqrt(1.0 - b2);
    double bp = bx*x[i] + by*y[i] + bz*z[i];
    double gamma2 = b2 > 0 ? (gamma - 1.0)/b2 : 0.0;

    //     SetX(X() + gamma2*bp*bx + gamma*bx*T());
    x[i] = x[i] + gamma2*bp*bx + gamma*bx*t[i];
    
    //SetY(Y() + gamma2*bp*by + gamma*by*T());
    y[i] = y[i] + gamma2*bp*by + gamma*by*t[i];

    //    SetZ(Z() + gamma2*bp*bz + gamma*bz*T());
    z[i] = z[i] + gamma2*bp*bz + gamma*bz*t[i];

    // SetT(gamma*(T() + bp));
    t[i] = gamma*(t[i] + bp);
  }
    
  Py_INCREF(Py_None);
  return Py_None;    
}

static PyObject* v4_gamma(PyObject *self, PyObject *args) {
  PyArrayObject *npin_x, *npin_y, *npin_z, *npin_t, *npout;
  int i, dims[2];
  double *xin, *yin, *zin, *tin, *out;

  if (!PyArg_ParseTuple(args, "O!O!O!O!", 
			&PyArray_Type, &npin_x, 
			&PyArray_Type, &npin_y, 
			&PyArray_Type, &npin_z,
			&PyArray_Type, &npin_t))
    return NULL;


  if (npin_x == NULL)
    return NULL;

  if (npin_y == NULL)
    return NULL;

  if (npin_z == NULL)
    return NULL;

  if (npin_t == NULL)
    return NULL;

  dims[0] = npin_x->dimensions[0];
  npout = (PyArrayObject*) PyArray_FromDims(1,dims,npin_x->descr->type_num);
  
  xin = (double*) npin_x->data;
  yin = (double*) npin_y->data;
  zin = (double*) npin_z->data;
  tin = (double*) npin_t->data;

  out = (double*) npout->data;

  for (i=0; i < dims[0]; i++) {
    double b = svc_v3_mag(xin[i], yin[i], zin[i]) / tin[i];
    out[i] = 1.0/sqrt(1 - b*b);
  }
  
  return PyArray_Return(npout);
}

/* 
 * Service function to return the magnitude of a single vector.
 */
double svc_v3_mag(double x, double y, double z) {
  return sqrt(x*x + y*y + z*z);
}

double svc_v3_mag2(double x, double y, double z) {
  return x*x + y*y + z*z;
}

double svc_v3_dot(double x1, double y1, double z1,
		  double x2, double y2, double z2)
{
  return x1*x2 + y1*y2 + z1*z2;
}

double svc_v3_cos_theta(double x, double y, double z) {
    return z / svc_v3_mag(x, y, z);  
}

double **npy2cptr_2d(PyArrayObject *arrayin)  {
	double **c, *a;
	int i,n,m;
	
	n=arrayin->dimensions[0];
	m=arrayin->dimensions[1];
	c=ptrvector(n);
	a=(double *) arrayin->data;  /* pointer to arrayin data as double */
	for ( i=0; i<n; i++)  {
		c[i]=a+i*m;  }
	return c;
}

double **ptrvector(long n)  {
	double **v;
	v=(double **)malloc((size_t) (n*sizeof(double)));
	if (!v) {
	  printf("In **ptrvector. Allocation of memory for double array failed.");
	  exit(0);  
	}

	return v;
}
